#include "astar.h"
#include <cassert>
#include <cstring>
#include <algorithm>

#define CALLBACK_BIND(__selector__,__target__, ...) std::bind(&__selector__,__target__, std::placeholders::_1, std::placeholders::_2, ##__VA_ARGS__)

static const int kStepValue = 10;
static const int kObliqueValue = 14;

AStar::AStar()
    : width_(0)
    , height_(0)
    , step_val_(kStepValue)
    , oblique_val_(kObliqueValue)
{
   // 构造函数
}

AStar::~AStar()
{
    clear();
}

// 获取直行估值
int AStar::get_step_value() const
{
    return step_val_;
}

// 获取拐角估值
int AStar::get_oblique_value() const
{
    return oblique_val_;
}

// 设置直行估值
void AStar::set_step_value(int value)
{
    step_val_ = value;
}

// 获取拐角估值
void AStar::set_oblique_value(int value)
{
    oblique_val_ = value;
}

// 清理参数
void AStar::clear()
{
    size_t index = 0;
    const size_t max_size = width_ * height_;
    
    while (index < max_size)
    {
        allocator_.free(mapping_[index++], sizeof(Node));
    }
    
    open_list_.clear();
    can_pass_ = nullptr;
    width_ = height_ = 0;
}

// 初始化操作
void AStar::init(const Params &param)
{
    width_ = param.width;
    height_ = param.height;
    can_pass_ = param.can_pass;
    if (!mapping_.empty())
    {
        memset(&mapping_[0], 0, sizeof(Node*) * mapping_.size());
    }
    mapping_.resize(width_ * height_, nullptr);
}

// 参数是否有效
bool AStar::is_vlid_params(const AStar::Params &param)
{
    return (param.can_pass != nullptr
            && (param.width > 0 && param.height > 0)
            && (param.end.x >= 0 && param.end.x < param.width)
            && (param.end.y >= 0 && param.end.y < param.height)
            && (param.start.x >= 0 && param.start.x < param.width)
            && (param.start.y >= 0 && param.start.y < param.height));
}

// 获取节点索引
bool AStar::get_node_index(Node *node, size_t *index)
{
    *index = 0;
    const size_t size = open_list_.size();
    
    while (*index < size)
    {
        if (open_list_[*index]->pos == node->pos)
        {
            return true;
        }
        ++(*index);
    }
    return false;
}

// 计算G值（通过 parent 到达 current 的 G 值）
inline uint16_t AStar::calcul_g_value(Node *parent, const Vec2 &current)
{
    uint16_t g_value = current.distance(parent->pos) == 2 ? oblique_val_ : step_val_;
    return g_value += parent->g;
}

// 计算F值
inline uint16_t AStar::calcul_h_value(const Vec2 &current, const Vec2 &end)
{
    unsigned int h_value = end.distance(current);
    return h_value * step_val_;
}

// 节点是否存在于开启列表
inline bool AStar::in_open_list(const Vec2 &pos, Node *&out_node)
{
    out_node = mapping_[pos.y * width_ + pos.x];
    return out_node ? out_node->state == IN_OPENLIST : false;
}

// 节点是否存在于关闭列表
inline bool AStar::in_closed_list(const Vec2 &pos)
{
    Node *node_ptr = mapping_[pos.y * width_ + pos.x];
    return node_ptr ? node_ptr->state == IN_CLOSEDLIST : false;
}

// 当前点是否可到达目标点(允许附近没有阻挡物的斜线)
bool AStar::can_pass_factor(const Vec2 &current, const Vec2 &destination)
{
    if (in_closed_list(destination))
    {
        return false;
    }
    
    if (destination.distance(current) == 1)
    {
        return can_pass_(destination);
    }
    else
    {
        return can_pass_(destination) && (can_pass_(Vec2(destination.x, current.y))
                                          && can_pass_(Vec2(current.x, destination.y)));
    }
}

// 当前点是否可到达目标点(允许所有斜线)
bool AStar::can_pass_all(const Vec2 &current, const Vec2 &destination)
{
    if (in_closed_list(destination))
    {
        return false;
    }
    
    return can_pass_(destination);
}

// 当前点是否可到达目标点(不允许斜线)
bool AStar::can_pass_only(const Vec2 &current, const Vec2 &destination)
{
    if (in_closed_list(destination))
    {
        return false;
    }
    
    if (destination.distance(current) == 1)
    {
        return can_pass_(destination);
    }
    return false;
}

// 查找附近可通过的节点
void AStar::find_can_pass_nodes(const Vec2 &current, std::vector<Vec2> *out_lists)
{
    Vec2 destination;
    
    int row_index = current.y - 1;
    int col_index = current.x - 1;
    int max_row = current.y + 1;
    int max_col = current.x + 1;

    if (row_index < 0)
    {
        row_index = 0;
    }
    
    if (col_index < 0)
    {
        col_index = 0;
    }
    
    if (max_row >= width_) {
        max_row = width_-1;
    }
    
    if (max_col >= height_) {
        max_col = height_-1;
    }
    
    while (row_index <= max_row)
    {
        for (int col = col_index; col <= max_col; col++) {
            destination.reset(col, row_index);
            
            if (find_pass_(current, destination))
            {
                out_lists->push_back(destination);
            }
        }
        
        ++row_index;
    }
}

// 处理找到节点的情况
void AStar::handle_found_node(Node *current, Node *destination)
{
    unsigned int g_value = calcul_g_value(current, destination->pos);
    if (g_value < destination->g)
    {
        destination->g = g_value;
        destination->parent = current;
        
        // 把重新指定父节点和G值的 destination 节点放到二叉堆正确的位置
        size_t index = 0;
        if (get_node_index(destination, &index))
        {
            if (open_list_[index]->f() < open_list_[0]->f())
            {
                std::swap(open_list_[index], open_list_[0]);
            }
        }
        else
        {
            assert(false);
        }
    }
}

// 处理未找到节点的情况
void AStar::handle_not_found_node(Node *current, Node *destination, const Vec2 &end)
{
    destination->parent = current;
    destination->h = calcul_h_value(destination->pos, end);
    destination->g = calcul_g_value(current, destination->pos);

    Node *&reference_node = mapping_[destination->pos.y * width_ + destination->pos.x];
    reference_node = destination;
    reference_node->state = IN_OPENLIST;

    // 将新产生的节点加入到开启列表
    open_list_.push_back(destination);
    
    // 对刚插入的元素顺序调整
    if (destination->f() < open_list_[0]->f())
    {
        std::swap(open_list_[open_list_.size()-1], open_list_[0]);
    }
}

// 执行寻路操作
std::vector<AStar::Vec2> AStar::find(const Params &param)
{
    std::vector<Vec2> paths;
    
    if (!is_vlid_params(param))
    {
        return paths;
    }

    // 初始化
    init(param);
    std::vector<Vec2> nearby_nodes;
    
    // 判断使用哪种规则
    switch (param.corner) {
        case 0: // 允许附近没有阻挡物的斜线
            nearby_nodes.reserve(8);
            find_pass_ = CALLBACK_BIND(AStar::can_pass_factor, this);
            break;
        case 1: // 允许所有斜线
            nearby_nodes.reserve(8);
            find_pass_ = CALLBACK_BIND(AStar::can_pass_all, this);
            break;
        case 2: // 不允许斜线
            nearby_nodes.reserve(4);
            find_pass_ = CALLBACK_BIND(AStar::can_pass_only, this);
            break;
        default:
            nearby_nodes.reserve(8);
            find_pass_ = CALLBACK_BIND(AStar::can_pass_factor, this);
            break;
    }
    
    // 将起点放入开启列表
    Node *start_node = new(allocator_.allocate(sizeof(Node))) Node(param.start);
    open_list_.push_back(start_node);
    Node *&reference_node = mapping_[start_node->pos.y * width_ + start_node->pos.x];
    reference_node = start_node;
    reference_node->state = IN_OPENLIST;

    // 寻路操作
    while (!open_list_.empty())
    {
        std::vector<Node*>::iterator it = open_list_.begin();
        
        // 找出f值最小节点为当前节点
        Node *current = *it;
        
        // 把当前节点从开启列表中移除
        open_list_.erase(it);
        
        // 保证最小f值在开启列表最后
        size_t open_count = open_list_.size();
        
        for (size_t i = 1; i < open_count; i++)
        {
            if (open_list_[i]->f() < open_list_[0]->f())
            {
                std::swap(open_list_[i], open_list_[0]);
            }
        }
        
        // 把当前节点加入到关闭列表中
        mapping_[current->pos.y * width_ + current->pos.x]->state = IN_CLOSEDLIST;

        // 是否找到终点
        if (current->pos == param.end)
        {
            // 从当前节点回溯父节点，直到回溯到起点，并把回溯路径加入队列，此路径就是最终寻路路径
            while (current->parent)
            {
                paths.push_back(current->pos);
                current = current->parent;
            }
            std::reverse(paths.begin(), paths.end());
            goto __end__;
        }

        // 查找周围可通过节点
        nearby_nodes.clear();
        find_can_pass_nodes(current->pos, &nearby_nodes);

        // 计算周围节点的估值
        size_t index = 0;
        const size_t size = nearby_nodes.size();
        while (index < size)
        {
            Node *next_node = nullptr;
            if (in_open_list(nearby_nodes[index], next_node))
            {
                handle_found_node(current, next_node);
            }
            else
            {
                next_node = new(allocator_.allocate(sizeof(Node))) Node(nearby_nodes[index]);
                handle_not_found_node(current, next_node, param.end);
            }
            ++index;
        }
    }

__end__:
    clear();
    return paths;
}
